import os
from launch import LaunchDescription,event_handlers
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription,ExecuteProcess,RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command,LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue
from ament_index_python.packages import get_package_share_directory



def generate_launch_description():
    #获取机器人urdf文件路径
    urdf_package_path = get_package_share_directory('robot_description')
    default_urdf_path = os.path.join(urdf_package_path,'urdf','robot','robot.urdf.xacro')

    #获取rviz设置文件路径
    # default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot.rviz')

    #获取gazebo设置文件路径
    default_gazebo_world_path = os.path.join(urdf_package_path,'world','custom_room.world')

    #声明urdf参数
    declare_urdf_model_path = DeclareLaunchArgument(
        name='model',
        default_value=default_urdf_path,
        description='加载urdf模型文件路径'
    )

    #从urdf路径中读取文件内容
    robot_description_value = ParameterValue(
        Command (['xacro ',LaunchConfiguration('model')]),
        value_type=str
    )

    #robot_state_publisher
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description":robot_description_value}],
        output="screen"
    )

    #joint_state_publisher
    # joint_state_publisher_node = Node(
    #     package="joint_state_publisher",
    #     executable="joint_state_publisher",
    #     output="screen"
    # )

    #rviz2
    # rviz_node = Node(
    #     package="rviz2",
    #     executable="rviz2",
    #     arguments=['-d',default_rviz_config_path],
    #     output="screen"
    # )

    #gazebo地图加载
    action_launch_gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={'world': default_gazebo_world_path, 'verbose': 'true'}.items()
    )

    #机器人加载到gezebo中
    action_spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=[
            '-topic', '/robot_description',
            '-entity', 'robot'
        ],
    )

    action_load_joint_state_controller = ExecuteProcess(
        cmd='ros2 control load_controller robot_joint_state_broadcaster --set-state active'.split(' '),
        output='screen'
    )

    # action__load_effort_controller = ExecuteProcess(
    #     cmd='ros2 control load_controller robot_effort_controller --set-state active'.split(' '),
    #     output='screen'
    # )
    action_load_diff_driver_controller = ExecuteProcess(
        cmd='ros2 control load_controller robot_diff_drive_controller --set-state active'.split(' '),
        output='screen'
    )

    return LaunchDescription([
        declare_urdf_model_path,
        robot_state_publisher_node,
        # joint_state_publisher_node,
        # rviz_node,
        action_launch_gazebo,
        action_spawn_entity,
        RegisterEventHandler(
            event_handler=event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller]
            )
        ),
        # RegisterEventHandler(
        #     event_handler=event_handlers.OnProcessExit(
        #         target_action=action_load_joint_state_controller,
        #         on_exit=[action__load_effort_controller]
        #     )
        # )
        RegisterEventHandler(
            event_handler=event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_diff_driver_controller]
            )
        )
    ])